#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>

#define PI (3.141592653589793)

/*
 * our_sobel: Daniel Lohmer 4/5/2008
 * CSE 30332
 * Program that takes in a source image and returns the magnitued and direction for the sobel operator.
 *  Order of derivative and box size can be specified.
 */
 
//
// void our_sobel(IplImage*, IplImage*, IplImage*, int, int, bool)
//
// Takes in pointers to the image source and two empty images of the same size.  Runs a sobel operator on the source
//  and then finds the direction and magnitude and puts them in the blank input images.  Order of derivative and the
//  size of the sobel operator can be specified, as can debug.
//
void our_sobel(IplImage * src, IplImage * magDest, IplImage * dirDest, int deriv=2, int boxSize=5, bool debug=0) {
  
 //ensure proper input
 if (!src || !magDest || !dirDest) {
    fprintf(stderr,"FAIL: src 0x%x magDest 0x%x dirDest 0x%x\n",src,magDest,dirDest);
    exit(-1);
 }
 if ((src->height != magDest->height) || (src->width != magDest->width) || (src->height != dirDest->height) || (src->width != dirDest->width)) {
    fprintf(stderr,"FAIL: image sizes mismatch.\n");
    exit(-1);
 }

 //create images to hold the vertical and horizontal sobel operators
 IplImage * dH = cvCreateImage(cvGetSize(src),8,1);
 cvZero(dH); //clear the image
 IplImage * dV = cvCreateImage(cvGetSize(src),8,1);
 cvZero(dV); //clear the image

 //run the sobel operators, first horizontally and then vertically and using user parameters or defaults
 cvSobel( src, dH, 0, deriv, boxSize );
 cvSobel( src, dV, deriv, 0, boxSize );

 //used for holding sobel, magnitude, and direction results
 int hd, vd;
 float mag,dir;
 
 //loop over the source image
 for (int i=0; i<src->height; i++) {
     for (int j=0; j<src->width; j++) {
         
         //get the intensity value at pixel (i,j)
         int hd = cvGetReal2D(dH, i, j);
         int vd = cvGetReal2D(dV, i, j);

         //calculate the magnitude, dividing by 2 to scale
         mag = sqrt(hd*hd+vd*vd)/2;
  
         //calculate the direction
         // use a function that finds atan2 (tangent half-angle formula) instead of atan to remove divide by zero problem
         // scale by 255/PI
         // if hd and vd are both zero, set direction to 0
         if (hd != 0 && vd != 0) dir = (510/PI) * atan(vd/(mag+hd));
         else dir = 0;
         
         //printf("Magnitude: %f\n", mag);
         //printf("Direction: %f\n", dir);
         
         //set the magnitude and direction pixels in the blank input images
         cvSetReal2D(magDest,i,j,mag);
         cvSetReal2D(dirDest,i,j,dir);
     }
 }

 //display results
 if (debug) {
    cvNamedWindow("Original image",CV_WINDOW_AUTOSIZE);
    cvShowImage("Original image",src);
    cvNamedWindow("Horizontal Sobel",CV_WINDOW_AUTOSIZE);
    cvShowImage("Horizontal Sobel",dH);
    cvNamedWindow("Vertical Sobel",CV_WINDOW_AUTOSIZE);
    cvShowImage("Vertical Sobel",dV);
    cvNamedWindow("Magnitude",CV_WINDOW_AUTOSIZE);
    cvShowImage("Magnitude",magDest);
    cvNamedWindow("Direction",CV_WINDOW_AUTOSIZE);
    cvShowImage("Direction",dirDest);
    cvWaitKey(0);
 }
 
 return;
}
